package ca.krakenpower.kvoxels;

public class Quaternion {
	
	public static final Quaternion ZERO = get(0, 0, 0, 0);
	
	public static Quaternion get(double a, double b, double c, double d) {
		return new Quaternion(a, b, c, d);
	}
	
	public static Quaternion get(double theta, Vector3 axis) {
		if (axis == null) {
			throw new IllegalArgumentException("null vector supplied as axis");
		}
		
		double sa = Math.cos(theta / 2);
		double t = Math.sin(theta / 2);
		double sb = axis.x * t;
		double sc = axis.y * t;
		double sd = axis.z * t;
		
		return new Quaternion(sa, sb, sc, sd);
	}
	
	public final double a, b, c, d;
	
	private Quaternion(double a, double b, double c, double d) {
		this.a = a;
		this.b = b;
		this.c = c;
		this.d = d;
	}
	
	public Quaternion hamilton(Quaternion p) {
		if (p == null) {
			throw new IllegalArgumentException("null quaternion supplied as operand for hamilton product");
		}
		
		double sa = (a * p.a) - (b * p.b) - (c * p.c) - (d * p.d);
		double sb = (a * p.b) + (b * p.a) + (c * p.d) - (d * p.c);
		double sc = (a * p.c) - (b * p.d) + (c * p.a) + (d * p.b);
		double sd = (a * p.d) + (b * p.c) - (c * p.b) + (d * p.a);
		
		return get(sa, sb, sc, sd);
	}
	
	public Quaternion product(Quaternion q) {
		if (q == null) {
			throw new IllegalArgumentException("null quaternion supplied as operand for product");
		}
		
		// (s + v)(t + w) = (st - v.w) + (sw + tv + v x w)
		double s, t, r;
		Vector3 v, w, u;
		s = this.scalarPart();
		t = q.scalarPart();
		v = this.vectorPart();
		w = q.vectorPart();
		
		r = (s * t) - v.dot(w);
		u = v.cross(w).sum(v.scale(t).sum(w.scale(s)));
		
		return new Quaternion(r, u.x, u.y, u.z);
	}
	
	public Quaternion conjugate() {
		return get(a, 0 - b, 0 - c, 0 - d);
	}
	
	public double scalarPart() {
		return a;
	}
	
	public Vector3 vectorPart() {
		return Vector3.get(b, c, d);
	}
	
	public Vector3 rotate(Vector3 v) {
		if (v == null) {
			throw new IllegalArgumentException("null vector supplied as operand for rotation");
		}
		
		Quaternion q = this;
		Quaternion p = get(0, v.x, v.y, v.z);
		Quaternion qs = this.conjugate();
		Quaternion pp = q.product(p).product(qs);
		
		return pp.vectorPart();
	}
	
	public double norm() {
		return Math.sqrt(a * a + b * b + c * c + d * d);
	}
	
	@Override
	public String toString() {
		return "[" + a + ", " + b + ", " + c + ", " + d + "]";
	}

}
